% IN:
%  indeks_robota - indeks robota, ktory powinien kontynuowac kolejkowanie
%
% OUT:
%  0 - kolejkowanie musi byc kontynuowane w kolejnej iteracji
%  1 - kolejkowanie zostalo zakonczone (robot znalazl sie w korytarzu i
%      jest w stanie 2)
%
% Zalozenia:
%  - robot znajduje sie w stanie 4 (kolejkowanie)
function ret = kontynuuj_kolejkowanie(indeks_robota)
    global roboty;
    global serwer_ob;
    global ilosc_robotow;
    global rozmiar_pola;
    global rozmiar_robota;
        
    indeks_korytarza=roboty(indeks_robota).indeks_korytarza;
    pozycja_w_kolejce = roboty(indeks_robota).pozycja_w_kolejce;
    pozycja = roboty(indeks_robota).pozycja;

    if(indeks_korytarza>0)
        % wjazd zachodni lub wschodni
        wjazd_korytarza = roboty(indeks_robota).pozycja(1) < serwer_ob.korytarze(indeks_korytarza,4).x;
    end;
    
    % robot nie jest juz w kolejce; jest w trakcie wjezdzania do korytarza
    if(pozycja_w_kolejce==0)
        
        wykonaj_ruch(indeks_robota);
        
        % okreslenie pierwszego pola w korytarzu do ktorego jedzie robot
        pole_korytarza = serwer_ob.korytarze(indeks_korytarza,(1-wjazd_korytarza)*7+1);
        pozycja = roboty(indeks_robota).pozycja;
        xy = roboty(indeks_robota).xy;
        
        % robot dotarl do pierwszego pola korytarza oraz zaglebil sie w nim
        % wystarczajco, aby zwolnic rezerwacje z pola poza korytarzem
        if(isequal(pozycja,[pole_korytarza.x pole_korytarza.y]))
            % spowoduje wjezdzanie robota do wnetrza korytarza, az nie
            % bedzie on "wystawal" na zewnatrz
            roboty(indeks_robota).droga=pozycja+[wjazd_korytarza 0];
            
            if( abs(rozmiar_pola/2-xy(1)) <= rozmiar_robota )
                roboty(indeks_robota).stan = 2; % poruszanie sie w korytarzu
                ret=1;
            end;
        end;
    % robot jest pierwszy w kolejce
    % i dojechal do docelowej pozycji:
    % wjazdu do korytarza lub kasy, w zaleznosci gdzie jedzie
    elseif(pozycja_w_kolejce==1 && ((indeks_korytarza>0 && isequal(roboty(indeks_robota).pozycja,wjazd_do_korytarza(indeks_korytarza, wjazd_korytarza))) || (indeks_korytarza==0 && isequal(roboty(indeks_robota).pozycja,serwer_ob.kasy(roboty(indeks_robota).numer_kasy,:)))))
        
        if(indeks_korytarza>0)
            docelowa_pozycja = wjazd_do_korytarza(indeks_korytarza, wjazd_korytarza);
        else
            docelowa_pozycja = serwer_ob.kasy(roboty(indeks_robota).numer_kasy,:);
        end;
        
        % robot stoi przed wjazdem do docelowego korytarza lub kasy
        if(isequal(pozycja,docelowa_pozycja))
            
            if(indeks_korytarza>0)
                % jesli robot jest obrocony na zachod/wschod (czyli przyjechal
                % do korytarza z naprzeciwka), to musi wykonac obrot na lewo,
                % aby stac przodem do wjazdu do korytarza
                if(roboty(indeks_robota).kierunek==2 || roboty(indeks_robota).kierunek==4)
                    roboty(indeks_robota).kierunek = roboty(indeks_robota).kierunek-1;
                end;

                % wyznaczenie drogi od pola poczatku kolejki do pierwszego
                % pola w korytarzu
                pole_do_przodu = kierunek_robota(indeks_robota,0);
                pole_na_prawo = kierunek_robota(indeks_robota,1);
                droga = [
                    pozycja+pole_do_przodu, 
                    pozycja+pole_do_przodu+pole_na_prawo
                    ];

                % proba zarezerwowania pol potrzebnych, aby wjechac do
                % korytarza
                if(rezerwuj(1,indeks_robota,droga)==1)
                    roboty(indeks_robota).droga=droga;
                    usun_z_kolejki(indeks_robota);

                    ret=0;
                end;
                ret=0;
            else
                roboty(indeks_robota).stan=6; % rozladunek
                ret=1;
            end;
        else
            % kontynuuj poruszanie sie do docelowego pola
            wykonaj_ruch(indeks_robota);
            ret=0;
        end;
    else % robot nie jest pierwszy w kolejce
        docelowe_pole = miejsce_w_kolejce(indeks_robota,pozycja_w_kolejce);
        cel = roboty(indeks_robota).cel;
        
        % docelowe pole zmienilo sie - poprzednik ruszyl sie
        if(~isequal(docelowe_pole,cel))
            dijkstry(pozycja,docelowe_pole,indeks_robota,0);
        end;
        
        % robot jeszcze nie osiagnal docelowego pola
        if(~isequal(pozycja,roboty(indeks_robota).cel))
            % zalozono, ze droga jest juz ustalona
            wykonaj_ruch(indeks_robota);
        end;
    end;
    
    %pozycja=roboty(indeks_robota).pozycja
    %droga=roboty(indeks_robota).droga(1:min(10,size(roboty(indeks_robota).droga,1)),:)
    %stan=roboty(indeks_robota).stan